%% --- 0. 清理工作区 ---
clear;
clc;
close all;

fprintf('--- 开始生成查找表 ---\n');

%% --- 1. 参数配置区 (请根据您的数据进行修改) ---

% 1.1 图像尺寸
image_width  = 188;
image_height = 120;

% 1.2 相机内参矩阵 K (从之前的标定结果复制)
K = [98.5693, 0,       98.3482;
     0,       98.7475, 56.5980;
     0,       0,       1.0000];

% 1.3 相机畸变参数 D (从之前的标定结果复制)
% D = [k1, k2, p1, p2]
D = [0.0153, -0.0222, 0, 0]; 

% 1.4 正向透视变换矩阵 H_inv (去畸变原图 -> 俯视图)
H_inv = [3.0652,    1.1802,   -2.0701;
         80.3454,   46.7404,  -80.5396;
         48.5323,   27.6441,  -47.6492];

% 1.5 逆向透视变换矩阵 H (俯视图 -> 去畸变原图)
H = [-0.7012,  -0.9906,    1.7048;
     -80.3738, -45.5885,   80.5482;
     -47.3437, -27.4575,   48.4461];

% 1.6 C代码格式化配置
output_filename_c = 'camera_maps_output.txt'; % 最终生成的C代码将保存在此文件中

fprintf('参数配置完成。\n');

%% --- 2. 正向打表：生成 mapx 和 mapy ---
% 作用：对于原图的每个像素，找到它在俯视图中的坐标

fprintf('正在生成正向查找表 (mapx, mapy)...\n');

% 创建一个 cameraIntrinsics 对象，方便后续函数调用
intrinsics = cameraIntrinsics(D, K', [image_height, image_width]);

% 创建原图所有像素的坐标网格
[u_distorted, v_distorted] = meshgrid(0:image_width-1, 0:image_height-1);
original_points = [u_distorted(:), v_distorted(:)];

% 对所有点进行去畸变
undistorted_points = undistortPoints(original_points, intrinsics);

% 将去畸变后的点应用透视变换 H_inv
homogeneous_points = [undistorted_points, ones(size(undistorted_points, 1), 1)]';
birdseye_homogeneous = H_inv * homogeneous_points;

% 从齐次坐标转回2D坐标
birdseye_x = birdseye_homogeneous(1, :) ./ birdseye_homogeneous(3, :);
birdseye_y = birdseye_homogeneous(2, :) ./ birdseye_homogeneous(3, :);

% 将一维数组重塑为二维的 mapx 和 mapy
mapx = reshape(birdseye_x, image_height, image_width);
mapy = reshape(birdseye_y, image_height, image_width);

fprintf('正向查找表生成完毕。\n');

%% --- 3. 逆向打表：生成 invx 和 invy ---
% 作用：对于俯视图的每个像素，找到它在原图中的坐标
% 注意：我们将逆向表的尺寸也设为与原图相同，即 120x188

fprintf('正在生成逆向查找表 (invx, invy)...\n');

% 创建俯视图的坐标网格 (尺寸与原图一致)
[x_bird, y_bird] = meshgrid(0:image_width-1, 0:image_height-1);
birdseye_points = [x_bird(:), y_bird(:)];

% 将俯视图坐标应用逆透视变换 H，得到在“去畸变原图”中的坐标
homogeneous_bird = [birdseye_points, ones(size(birdseye_points, 1), 1)]';
undistorted_homogeneous = H * homogeneous_bird;

% 从齐次坐标转回2D坐标
undistorted_x = undistorted_homogeneous(1, :) ./ undistorted_homogeneous(3, :);
undistorted_y = undistorted_homogeneous(2, :) ./ undistorted_homogeneous(3, :);

% 将去畸变坐标“重新畸变”回原始图像坐标
% 注意：distortPoints 函数在 Computer Vision Toolbox 中可能不存在
% 我们需要使用 cameraIntrinsics 对象的内部方法，或者手动计算
% 这里我们使用一种更通用的方法：插值
% 我们已经有了从原图到去畸变图的完整映射，现在反向查找

% 创建一个插值对象
[orig_u, orig_v] = meshgrid(0:image_width-1, 0:image_height-1);
F_u = griddedInterpolant(undistorted_points(:,1), undistorted_points(:,2), original_points(:,1), 'linear', 'none');
F_v = griddedInterpolant(undistorted_points(:,1), undistorted_points(:,2), original_points(:,2), 'linear', 'none');

distorted_x = F_u(undistorted_x, undistorted_y);
distorted_y = F_v(undistorted_x, undistorted_y);

% 将结果重塑为 invx 和 invy，并处理NaN值（超出原始图像范围的点）
invx = reshape(distorted_x, image_height, image_width);
invy = reshape(distorted_y, image_height, image_width);

% 将 NaN 替换为0，或一个特定的无效值
invx(isnan(invx)) = 0; 
invy(isnan(invy)) = 0;

% 转换为整数类型，因为像素坐标是整数
invx = int32(round(invx));
invy = int32(round(invy));

fprintf('逆向查找表生成完毕。\n');

%% --- 4. 将结果导出为 C/C++ 头文件格式 ---

fprintf('正在将所有查找表导出到文件: %s\n', output_filename_c);

fid = fopen(output_filename_c, 'w');

% --- 导出 mapx ---
fprintf(fid, '// 正向查找表 (原图x -> 俯视x)\n');
fprintf(fid, 'float mapx[%d][%d] = {\n', image_height, image_width);
for i = 1:image_height
    fprintf(fid, '{');
    for j = 1:image_width
        fprintf(fid, '%.4f', mapx(i, j));
        if j < image_width
            fprintf(fid, ', ');
        end
    end
    fprintf(fid, '}');
    if i < image_height
        fprintf(fid, ',\n');
    end
end
fprintf(fid, '\n};\n\n');

% --- 导出 mapy ---
fprintf(fid, '// 正向查找表 (原图y -> 俯视y)\n');
fprintf(fid, 'float mapy[%d][%d] = {\n', image_height, image_width);
for i = 1:image_height
    fprintf(fid, '{');
    for j = 1:image_width
        fprintf(fid, '%.4f', mapy(i, j));
        if j < image_width
            fprintf(fid, ', ');
        end
    end
    fprintf(fid, '}');
    if i < image_height
        fprintf(fid, ',\n');
    end
end
fprintf(fid, '\n};\n\n');

% --- 导出 invx ---
fprintf(fid, '// 逆向查找表 (俯视 -> 原图x)\n');
fprintf(fid, 'int invx[%d][%d] = {\n', image_height, image_width);
for i = 1:image_height
    fprintf(fid, '{');
    for j = 1:image_width
        fprintf(fid, '%d', invx(i, j));
        if j < image_width
            fprintf(fid, ', ');
        end
    end
    fprintf(fid, '}');
    if i < image_height
        fprintf(fid, ',\n');
    end
end
fprintf(fid, '\n};\n\n');

% --- 导出 invy ---
fprintf(fid, '// 逆向查找表 (俯视 -> 原图y)\n');
fprintf(fid, 'int invy[%d][%d] = {\n', image_height, image_width);
for i = 1:image_height
    fprintf(fid, '{');
    for j = 1:image_width
        fprintf(fid, '%d', invy(i, j));
        if j < image_width
            fprintf(fid, ', ');
        end
    end
    fprintf(fid, '}');
    if i < image_height
        fprintf(fid, ',\n');
    end
end
fprintf(fid, '\n};\n\n');

fclose(fid);

fprintf('--- 全部完成！请检查 %s 文件。---\n', output_filename_c);